#include <hnurm_uav/hnurm_uav.hpp>
HNURM_UAV_Node::HNURM_UAV_Node(ros::NodeHandle& nh, ros::NodeHandle& nh_private)
{

    RemoteControler_sub = nh.subscribe<sensor_msgs::Joy>("/dji_sdk/rc", 1, &HNURM_UAV_Node::Remote_controller_callback, this);
    height_sub = nh.subscribe<std_msgs::Float32>("/dji_sdk/height_above_takeoff",10,&HNURM_UAV_Node::height_sub_callback, this);
    flightStatusSub = nh.subscribe("dji_sdk/flight_status", 10, &HNURM_UAV_Node::flight_status_callback,this);
    displayModeSub = nh.subscribe("dji_sdk/display_mode", 10, &HNURM_UAV_Node::display_mode_callback,this);
    attitudeSub = nh.subscribe("dji_sdk/attitude", 10, &HNURM_UAV_Node::attitude_callback,this);

    //publisher
    ctrlPosYawPub = nh.advertise<sensor_msgs::Joy>("dji_sdk/flight_control_setpoint_ENUposition_yaw", 10);

    timer_ = nh.createTimer(ros::Duration(0.01),&HNURM_UAV_Node::timer_callback,this);
    // Basic services
    sdk_ctrl_authority_service = nh.serviceClient<dji_sdk::SDKControlAuthority> ("dji_sdk/sdk_control_authority");
    drone_task_service         = nh.serviceClient<dji_sdk::DroneTaskControl>("dji_sdk/drone_task_control");
    query_version_service      = nh.serviceClient<dji_sdk::QueryDroneVersion>("dji_sdk/query_drone_version");
    set_local_pos_reference    = nh.serviceClient<dji_sdk::SetLocalPosRef> ("dji_sdk/set_local_pos_ref");
    localPosition_sub = nh.subscribe<nav_msgs::Odometry>("/camera/odom/sample",10,&HNURM_UAV_Node::local_position_callback, this);
    // obtain_control_result = obtain_control();
    // if (!set_local_position()) // We need this for height
    // {
    //     ROS_ERROR("GPS health insufficient - No local frame reference for height. Exiting.");
    // }




}

HNURM_UAV_Node::~HNURM_UAV_Node(){
    return;
}
bool last_state = true;
void HNURM_UAV_Node::monitorControlPermissions()
{   if(RC_state == RemoteControlState::Disconnected) return;
    if(RCcontrol.load())
    {
        if(last_state == RCcontrol.load()) return;
        last_state  = RCcontrol.load();
        release_control();
    }
    else
    {
        if(last_state == RCcontrol.load()) return;
        last_state  = RCcontrol.load();
        obtain_control();
    }

}

void HNURM_UAV_Node::flight_status_callback(const std_msgs::UInt8::ConstPtr& msg)
{
    flight_status = msg->data;

}

void HNURM_UAV_Node::display_mode_callback(const std_msgs::UInt8::ConstPtr& msg){
    display_mode = msg->data;
}

geometry_msgs::Vector3 toEulerAngle(geometry_msgs::Quaternion quat)
{
  geometry_msgs::Vector3 ans;

  tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
  R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);
  return ans;
}

void HNURM_UAV_Node::attitude_callback(const geometry_msgs::QuaternionStamped::ConstPtr& msg)
{
    current_atti = toEulerAngle(msg->quaternion).z;
}



void HNURM_UAV_Node::Remote_controller_callback(const sensor_msgs::Joy::ConstPtr& msg)
{
    if((msg->axes[0]==-1.0)&&(msg->axes[1]==-1.0)&&(msg->axes[2]==-1.0)&&(msg->axes[3]==-1.0)&&(msg->axes[4]==0.0)&&(msg->axes[5]==0.0))
    {
      RC_state = RemoteControlState::Disconnected;
    }
    else if((msg->axes[0]==0.0)&&(msg->axes[1]==0.0)&&(msg->axes[2]==0.0)&&(msg->axes[3]==0.0)&&(msg->axes[4]==0.0)&&(msg->axes[5]==0.0))
    {
      RC_state = RemoteControlState::Centered;
        RCcontrol.store(false);
    }
    else
    {
    RCcontrol.store(true);
    RC_state = RemoteControlState::Controlling;
    RC_control = *msg;
    RC_control.axes[0] = msg->axes[1];
    RC_control.axes[1] = msg->axes[0];
    // ctrlPosYawPub.publish(RC_control);
    ROS_WARN("Remote controller is controlling the uav");
    }
    // switch(RC_state){
    //     case RemoteControlState::Disconnected:
    //         ROS_WARN("Remote is Disconnected");
    //         break;
    //     case RemoteControlState::Centered:
    //         ROS_WARN("Remote is Centered");
    //         break;
    //     case RemoteControlState::Controlling:
    //         ROS_WARN("Remote is Controlling");
    //         break;
    // }
}
/*
@brief
   RC_state == Centered 时记录下当前的摇杆回中的pose，如果状态没有发生切换，
   保持hover_pos不变
   当操作摇杆的时候，继续更新当前pose
*/
void HNURM_UAV_Node::height_sub_callback(const std_msgs::Float32::ConstPtr& msg)
{
    current_height = msg->data;
}

void HNURM_UAV_Node::local_position_callback(const nav_msgs::Odometry::ConstPtr& msg)
{
    static ros::Time start_time = ros::Time::now();
    ros::Duration elapsed_time = ros::Time::now() - start_time;
    if(RC_state == RemoteControlState::Centered && monitoredTakeoff())
    {
        hover.pos_x = current.pos_x;
        hover.pos_y = current.pos_y;
        hover.pos_z = current.pos_z;
        // ROS_WARN("hover pose: x= %f  y= %f  z= %f",hover.pos_x,hover.pos_y,hover.pos_z);
        // 计算当前位置与目标位置的偏差
        double error_x = hover.pos_x -  msg->pose.pose.position.x;
        double error_y = hover.pos_y - msg->pose.pose.position.y;
        double error_z = hover.pos_z - msg->pose.pose.position.z;
        // PD 控制器输出
        double xCmd = Kp_x * error_x + Kd_x * (error_x - prev_error_x)  / dt;
        double yCmd = Kp_y * error_y + Kd_y * (error_y - prev_error_y)  / dt;
        double zCmd = hover.pos_z;
        if(zCmd>=3.5) zCmd = 2.5;
        ROS_WARN("cmd msg : x= %f  y= %f  z= %f",xCmd, yCmd, zCmd);
        // 更新上一时刻偏差
        prev_error_x = error_x;
        prev_error_y = error_y;
        prev_error_z = error_z;

        // hover = current;
        // double xCmd = hover.pos_x - msg->pose.pose.position.x;
        // double yCmd = hover.pos_y - msg->pose.pose.position.y;
        // // double zCmd = hover.pos_z - current_height;
        // double zCmd = hover.pos_z - msg->pose.pose.position.z;

        double yawCmd = hover.pos_yaw - current_atti;
        // Down sampled to 50Hz loop
        if (elapsed_time > ros::Duration(0.02)) {
            start_time = ros::Time::now();
        local_position_ctrl(xCmd, yCmd, zCmd,yawCmd);
        }
    }
    else if(RC_state == RemoteControlState::Controlling)
    {
        current.pos_x = msg->pose.pose.position.x;
        current.pos_y = msg->pose.pose.position.y;
        // current.pos_z = current_height;
        current.pos_z = msg->pose.pose.position.z;
        current.pos_yaw = current_atti;
    }
}

void HNURM_UAV_Node::timer_callback(const ros::TimerEvent&)
{
    monitorControlPermissions();
}

bool HNURM_UAV_Node::set_local_position(){

  dji_sdk::SetLocalPosRef localPosReferenceSetter;
  set_local_pos_reference.call(localPosReferenceSetter);
  return localPosReferenceSetter.response.result;

}



bool HNURM_UAV_Node::obtain_control(){
    dji_sdk::SDKControlAuthority authority;
    authority.request.control_enable=1;
    sdk_ctrl_authority_service.call(authority);

    if(!authority.response.result)
    {
        ROS_ERROR("obtain control failed!");
        return false;
    }
    return true;
}

bool HNURM_UAV_Node::release_control(){
    dji_sdk::SDKControlAuthority authority;
    authority.request.control_enable=0;
    sdk_ctrl_authority_service.call(authority);

    if(!authority.response.result)
    {
        ROS_ERROR("release control failed!");
        return false;
    }

    return true;
}


bool HNURM_UAV_Node::is_M100(){
    dji_sdk::QueryDroneVersion query;
    query_version_service.call(query);

    if(query.response.version == DJISDK::DroneFirmwareVersion::M100_31)
    {
        return true;
    }

    return false;
}

bool HNURM_UAV_Node::monitoredTakeoff(){
    /*
     *this function is diffrent from the dji sdk monitoredTakeoff function
     *for indoor hover, we don't use auto takeoff, instead , artificial autonomous control
     *can help pilot handle emergency
     *general logic:
     *      spin the motor
     *             |
     *             V
     *      get in to the air
     *             |
     *             V
     * final check: finish artificial takeoff   --->  check : is landed?
     *             |
     *             V
     *    in the air
     *      monitor the RC state
     *    ->  if centered : hover  else : RC control dominant the fc
     *
     * THIS FUNCTION WOULD NOT ONLY SPIN ONCE
     * in the whole game , uav would takeoff and land for many times ,this function
     * would not spin for only once , we should monitor current  DJISDK::FlightStatus
     * to determine the next step
     *
     *
     *
     *
    */
    // Step 1.2: Get in to the air    display_mode == MODE_AUTO_TAKEOFF(6)  FlightStatus == STATUS_IN_AIR(2)
    if (flight_status == 2) {
        ROS_WARN("Aircraft is in the air !!");
        return true;
    }
    else if (flight_status==1 && display_mode == 10)
     {
        ROS_WARN("waiting for taking off!!");
        return false;
    }
    else if ((flight_status==0 && display_mode == 17)||(flight_status==0 && display_mode == 6))
     {
        ROS_WARN("Aircraft landed,waiting for taking off!!");
        return  false;
    }
    return false;
}



void HNURM_UAV_Node::local_position_ctrl(double &xCmd, double &yCmd, double &zCmd ,double &yawCmd)
{
  sensor_msgs::Joy controlPosYaw;
  controlPosYaw.axes.push_back(xCmd);
  controlPosYaw.axes.push_back(yCmd);
  controlPosYaw.axes.push_back(zCmd);
//   controlPosYaw.axes.push_back(yawCmd);
  ctrlPosYawPub.publish(controlPosYaw);

  // 0.1m or 10cms is the minimum error to reach target in x y and z axes.
  // This error threshold will have to change depending on aircraft/payload/wind conditions.
}


